/*
 * @Description: 麦克纳姆轮运动控制
 * @Author: Dryad
 * @Date: 2019-08-02 12:45:34
 */

#pragma once
#include <arm_math.h>
#include "../Applications/Coordinate_Velocity.hpp"
#include "./ARES-8015-E-AC.hpp"

class Mecanum_AGV_Class
{
public:
    static void Init(float distance_x, float distance_y, float max_velocity, float max_rpm); /* 设置初始参数和硬件 */
    static void Write_Velocity(const Velocity_Class &velocity_inagv);                        /* 控制AGV按照指定速度运动 */
    static void Read_Velocity(float (&velocity)[4]);                                         /* 获取4个车轮的线速度，分别保存第一、二、三、四象限的速度 */

    static void Set_Motor_Mode(Motor_Class::Motor_State_Enum state) { Motor_Class::Set_State(state); }; /* 设置电机模式 */

    /* 
    *   读取电机模式
    *   bit[0..3]   第一象限电机
    *   bit[4..7]   第二象限电机
    *   bit[8..11]  第三象限电机
    *   bit[12..15] 第四象限电机
    */
    static unsigned short Read_Motor_Mode(void);
    static void Write_Zero_Velocity(void); /* 设置电机零速 */
    static void Clear_Wrong_Code(void);    /* 清除错误代码 */

private:
    static arm_matrix_instance_f32 A_matrix, v_agv_matrix, v_motor_matrix; /* 麦轮逆运动学矩阵 */
    static float A_matrix_data[12], agv_v_data[3], motor_v_data[4];
};
